/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Service implementation of Hub Name Service.
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#define _GNU_SOURCE
#include <string.h>
#include "protocolDispatcher.h"
#include "protocol/fastProtocolHdr.h"
#include "protocol/hubctrlHdr.h"
#include "apmulticastframe.h"
#include "owndebug.h"
#include "ownutils.h"
#include <signal.h>
#include <curses.h>
#include <assert.h>

//*****************************************************************************
//*****************************************************************************
#define GIMNETAP_MULTICAST_TESTAPP_VERSIONSTRING "v0.0-Alpha"
//*****************************************************************************
//*****************************************************************************

static volatile int run = 1;
static int verbose = ODINFO;
static ownThreadHandle signalHandlerThread = NULL;
static ownThreadHandle protocolDispProcessorThreadHandle = NULL;
static ownMutexHandle lock;
static ownCondHandle cond;
static WINDOW *w = NULL;
//*****************************************************************************
static uint64_t owner_key = 0x00000000;
static uint64_t access_key = 0x00000000;
static uint32_t lastRegisteredMulticastId = 0x0000000;
static uint32_t targetNodeId = 0x00010002;
static uint32_t localNodeId = 0x00000000;
static uint32_t messageSeq = 0;
static uint32_t lastReceivedMessageSeq_async = 0;
static TProtocolDisp aplink;
static char mcast_payload[4096] = "";
static size_t mcast_payload_size = 0;

//*****************************************************************************
unsigned char g_replybuffer[sizeof(TFastProtocolHeader)+sizeof(TAPMulticastFrame)+sizeof(TAPMulticastCtrl)];
TFastProtocolHeader *g_replyfpframe = 
  (TFastProtocolHeader *)g_replybuffer;
TAPMulticastFrame *g_replymcframe = 
  (TAPMulticastFrame *)(g_replybuffer+sizeof(TFastProtocolHeader));
TAPMulticastCtrl *g_replymcctrl = 
  (TAPMulticastCtrl *)(g_replybuffer+sizeof(TFastProtocolHeader)+sizeof(TAPMulticastFrame));
//*****************************************************************************
//*****************************************************************************

static void Lock()
{
  ownMutex_Lock(lock);
}
//*****************************************************************************

static void Unlock()
{
  ownMutex_Unlock(lock);
}
//*****************************************************************************

static void notifyReply(const int id)
{
  Lock();
  lastReceivedMessageSeq_async = id;
  ownCond_Broadcast(cond);
  Unlock();
}
//*****************************************************************************

static int waitReply(const int id, const int timeout)
{
  int result = 0;
  ownTime_ms_t begin = ownTime_get_ms();
  ownTime_ms_delta_t left = timeout;

  Lock();
  while(lastReceivedMessageSeq_async < id && left > 0) {
    ownCond_Wait(cond, lock, left);
    left = ownTime_get_ms_left(timeout, begin);
  }
  
  result = (lastReceivedMessageSeq_async >= id);
  Unlock();
  
  return result;
}
//*****************************************************************************

static int protocolDispProcessorThread(void *p)
{
  TProtocolDisp *link = (TProtocolDisp *)p;
  while(run) {
    if (protocolDispProcess(link, 1000)) {
      dPrint(ODTEST,"Process returned error!");
      run = 0;
    }
  }
  return 0;
}
//*****************************************************************************

static void processFastProtocolFrame(const TFastProtocolHeader *framehdr,
                                     const unsigned char *payload,
                                     const unsigned int payloadSize,
                                     void *userPtr)
{  
  /*  dPrint(ODTEST,"FastProtocolFrame: %p, %p, %d",
      framehdr, payload, payloadSize);*/
  
  // Check that payload size is atleast size of the main frame
  if (payloadSize == sizeof(TAPMulticastFrame)+sizeof(TAPMulticastCtrl) &&
      (framehdr->flags & (FASTPROTOCOL_FLAG_STATUS | FASTPROTOCOL_FLAG_ERROR)) == 0 &&
      ((const TAPMulticastFrame *)payload)->service_id == GIMNETAP_SERVICE_MULTICAST_ID) {
    const TAPMulticastFrame *mcframe = (const TAPMulticastFrame *)payload;
    const TAPMulticastCtrl *mcctrl = (const TAPMulticastCtrl *)(payload+sizeof(TAPMulticastFrame));
    //    const TAPMulticastPayload *mcpayl = (const TAPMulticastPayload *)(payload+sizeof(TAPMulticastFrame));
    
    if (mcframe->flags & KAPMulticastFlagReply) {
      if (mcframe->request == KAPMulticastRequestCreateMulticastId) {
        lastRegisteredMulticastId = mcctrl->multicast_id;
        printw("\rCreating new MulticastId completed, received MulticastID 0x%08lx (Operation was a %s)\n",
               mcctrl->multicast_id,
               mcctrl->result==KAPMulticastResultOK?"success!":"failure!");
        
      } else if (mcframe->request == KAPMulticastRequestPublishPublic) {  
        printw("\rPublish Public Request for MulticastId 0x%08lx completed. (Operation was a %s)\n",
               mcctrl->multicast_id,
               mcctrl->result==KAPMulticastResultOK?"success!":"failure!");
        
        
      } else if (mcframe->request == KAPMulticastRequestSubscribe) {  
        printw("\rSubscribe to MulticastId 0x%08lx completed. (Operation was a %s)\n",
               mcctrl->multicast_id,
               mcctrl->result==KAPMulticastResultOK?"success!":"failure!");
        
        
      } else {
        printw("\rUnprocessed Multicast reply with requestId = 0x%02x, resultCode = 0x%02x\n",
               mcframe->request,
               mcctrl->result); 
        
      }

      notifyReply(mcframe->identifier);
    }
    
  } else if ((framehdr->flags & (FASTPROTOCOL_FLAG_STATUS | FASTPROTOCOL_FLAG_ERROR)) == 0) {
    printw("\rReceived Non-Multicast Service data, %u bytes, from 0x%08x\n",
           payloadSize, framehdr->source_id);
    
  } else {
    printw("\rReceived FastProtocol control data. Unprocessed at the moment.\n");
  }
}
//*****************************************************************************

static void hubctrlProtocolCallback(int protocol_number,
                                    const unsigned char *data,
                                    int size,
                                    void *userPtr)
{
  // This function gets called when a HubCtrlProtocol packet is received
  // from Hub.
  const THubCtrlProtocolHeader *frame = (const THubCtrlProtocolHeader *)data;
  
  if (size >= sizeof(THubCtrlProtocolHeader) &&
      size == frame->protocol_id.total_size) {
    // Protocol size match; We have enough data to process call
    if (frame->request == KHubCtrlRequestQueryLocalNodeId &&
        frame->flags & KHubCtrlFlagReply &&
        size == sizeof(THubCtrlProtocolHeader)+sizeof(THubCtrlSubHeaderNodeInfo)) {
      THubCtrlSubHeaderNodeInfo *ni = (THubCtrlSubHeaderNodeInfo *)(frame+1);
      
      printw("\rOwn address for this module is '0x%08x' (Link delay %u us)\n",
             ni->nodeid, ni->link_delay_us);
      localNodeId = ni->nodeid;
      
      // Update fp source (not really required as Hub overrides)
      g_replyfpframe->source_id = localNodeId;
    }
    
  } else {
    dPrint(ODWARN,"Warning: Received invalid HubCtrlProtocol frame!");
    
  }
}
//*****************************************************************************

static void fastProtocolCallback(int protocol_number,
                                 const unsigned char *data,
                                 int size,
                                 void *userPtr)
{
  // This function gets called when a FastProtocol packet is received
  // from Hub.
  const TFastProtocolHeader *frame = (const TFastProtocolHeader *)data;

  if (size >= sizeof(TFastProtocolHeader) &&
      size == frame->protocol_id.total_size) {
    // Protocol size match; We have enough data to process call
    processFastProtocolFrame(frame,
                             data+sizeof(TFastProtocolHeader),
                             size-sizeof(TFastProtocolHeader),
                             userPtr);

  } else {
    dPrint(ODWARN,"Warning: Received invalid FastProtocol frame!");
    
  }
}
//*****************************************************************************


static void requestOwnId(TProtocolDisp *link)
{
  THubCtrlProtocolHeader frame;

  // Clear header.
  memset(&frame, 0x00, sizeof(frame));
  
  // Assign identification information
  protocolIdentificationConstructHeader(&frame.protocol_id,
                                        HUBCTRLPROTOCOL_NUMBER,
                                        sizeof(THubCtrlProtocolHeader));
  
  // Copy parameters
  frame.request = KHubCtrlRequestQueryLocalNodeId;
  frame.flags = 0x00;
  frame.subheader_count = 0;
  frame.result = 0;
  frame.message_id = ++messageSeq;
  
  protocolDispSendPacket(link,(void*)&frame,sizeof(frame));
}
//*****************************************************************************

static void showwait()
{
  const char value[] = "|/-\\";
  static int phase = 0;
  printw("\r %c: ", value[phase]);
  phase = (phase + 1) % 4;
}
//*****************************************************************************

static void test_TestSeq1(void)
{
  // Construct Query
  {
    const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                        KAPMulticastRequestCreateMulticastId,
                                        0x0000,
                                        ++messageSeq};
    const TAPMulticastCtrl mcctrl = { 0x00, // Multicast ID
                                      owner_key,
                                      0x00, // Access key
                                      0x00 }; // Result
    
    *g_replymcframe = mcframe;
    *g_replymcctrl = mcctrl;
    protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
    if (!waitReply(messageSeq, 1000)) 
      printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
             __FUNCTION__, messageSeq);
  }
  {
    // Construct Query
    const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                        KAPMulticastRequestPublishPublic,
                                        0x0000,
                                        ++messageSeq };
    const TAPMulticastCtrl mcctrl = { lastRegisteredMulticastId, // Multicast ID
                                      owner_key,
                                      access_key, // Access key
                                      0x00 }; // Result
    
    *g_replymcframe = mcframe;
    *g_replymcctrl = mcctrl;
    protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
    if (!waitReply(messageSeq, 1000)) 
      printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
             __FUNCTION__, messageSeq);     
  }
  {
    // Construct Query
    const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                        KAPMulticastRequestSubscribe,
                                        0x0000,
                                        ++messageSeq  };
    const TAPMulticastCtrl mcctrl = { lastRegisteredMulticastId, // Multicast ID
                                      0x00,
                                      access_key, // Access key
                                      0x00 }; // Result
    
    *g_replymcframe = mcframe;
    *g_replymcctrl = mcctrl;
    protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
    if (!waitReply(messageSeq, 1000)) 
      printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
             __FUNCTION__, messageSeq);      
  }
  { 
    static unsigned int repeat = 1;
    static unsigned int interval_ms = 100;
    
    // Construct Query
    const unsigned int multicast_testbuffer_size = ( sizeof(TFastProtocolHeader) + 
                                                     sizeof(TAPMulticastFrame) + 
                                                     sizeof(TAPMulticastPayload) + 
                                                     mcast_payload_size );
    uint8_t * multicast_testbuffer = (uint8_t *) malloc(multicast_testbuffer_size);
    assert(multicast_testbuffer != NULL);
    
    
    TFastProtocolHeader *fpframe = (TFastProtocolHeader*)multicast_testbuffer;
    TAPMulticastFrame *mcframe = (TAPMulticastFrame *)(multicast_testbuffer+sizeof(TFastProtocolHeader));
    TAPMulticastPayload *mcpl = (TAPMulticastPayload *)(multicast_testbuffer+sizeof(TAPMulticastFrame)+sizeof(TFastProtocolHeader));
    
    
    // Setup FastProtocolFrame
    fpframe->source_id = 0x00;
    fpframe->target_id = targetNodeId;
    fpframe->flags = 0x00;
    fpframe->ttl = FASTPROTOCOL_DEFAULT_TTL;
    fpframe->statusvalue = 0x00;
    fpframe->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
    fpframe->protocol_id.total_size = multicast_testbuffer_size;
    
    // Setup MulticastFrame
    mcframe->service_id = GIMNETAP_SERVICE_MULTICAST_ID;
    mcframe->request = KAPMulticastRequestMulticast;
    
    // Setup payload header
    mcpl->multicast_id = lastRegisteredMulticastId;
    mcpl->access_key = access_key;
    
    // copy payload
    assert(mcast_payload_size < sizeof(mcast_payload));
    memcpy(mcpl->payload, mcast_payload, mcast_payload_size);
    
    // Send!
    int repeat_count;
    for(repeat_count=0; repeat_count < repeat; ++repeat_count) {
      if (repeat_count > 0) ownSleep_ms(interval_ms);
    printw("Sending FastProtocol frame total of %u bytes long\n", multicast_testbuffer_size);
    protocolDispSendPacket(&aplink,(void*)multicast_testbuffer,multicast_testbuffer_size);
    }
  }
}
//*****************************************************************************

static void test_CreateMulticastId(void)
{
  printw("Creating new MulticastID:\n");
  printw("Owner_Key? [0x%016llx]: ", owner_key);
  scanw("%llx", &owner_key);
  halfdelay(1);

  const double begin = ownTime_get_s();
  
  // Construct Query
  const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                      KAPMulticastRequestCreateMulticastId,
                                      0x0000,
                                      ++messageSeq };
  const TAPMulticastCtrl mcctrl = { 0x00, // Multicast ID
                                    owner_key,
                                    0x00, // Access key
                                    0x00 }; // Result
  
  *g_replymcframe = mcframe;
  *g_replymcctrl = mcctrl;
  protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     
  
  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin)); 
}
//*****************************************************************************

static void test_PublishPublic(void)
{
  printw("Publish Public:\n");
  printw("MulticastId? [0x%08x]: ", lastRegisteredMulticastId);
  scanw("%lx", &lastRegisteredMulticastId);

  printw("Owner_Key? [0x%016llx]: ", owner_key);
  scanw("%llx", &owner_key);

  printw("Access_Key? [0x%016llx]: ", access_key);
  scanw("%llx", &access_key);

  halfdelay(1);

  const double begin = ownTime_get_s();
  
  // Construct Query
  const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                      KAPMulticastRequestPublishPublic,
                                      0x0000,
                                      ++messageSeq};
  const TAPMulticastCtrl mcctrl = { lastRegisteredMulticastId, // Multicast ID
                                    owner_key,
                                    access_key, // Access key
                                    0x00 }; // Result
  
  *g_replymcframe = mcframe;
  *g_replymcctrl = mcctrl;
  protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     
  
  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));  
}
//*****************************************************************************

static void test_PublishPrivate(void)
{
  printw("Publish Private:\n");
  printw("MulticastId? [0x%08x]: ", lastRegisteredMulticastId);
  scanw("%lx", &lastRegisteredMulticastId);
  
  printw("Owner_Key? [0x%016llx]: ", owner_key);
  scanw("%llx", &owner_key);
  
  printw("Access_Key? [0x%016llx]: ", access_key);
  scanw("%llx", &access_key);
  
  halfdelay(1);
  
  const double begin = ownTime_get_s();
  
  // Construct Query
  const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                      KAPMulticastRequestPublishPrivate,
                                      0x0000,
                                      ++messageSeq};
  const TAPMulticastCtrl mcctrl = { lastRegisteredMulticastId, // Multicast ID
                                    owner_key,
                                    access_key, // Access key
                                    0x00 }; // Result
  
  *g_replymcframe = mcframe;
  *g_replymcctrl = mcctrl;
  protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     
  
  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));  
  
}
//*****************************************************************************

static void test_Subscribe(void)
{
  printw("Subscribe:\n");
  printw("MulticastId? [0x%08x]: ", lastRegisteredMulticastId);
  scanw("%lx", &lastRegisteredMulticastId);

  printw("Access_Key? [0x%016llx]: ", access_key);
  scanw("%llx", &access_key);

  halfdelay(1);

  const double begin = ownTime_get_s();
  
  // Construct Query
  const TAPMulticastFrame mcframe = { GIMNETAP_SERVICE_MULTICAST_ID,
                                      KAPMulticastRequestSubscribe,
                                      0x0000,
                                      ++messageSeq};
  const TAPMulticastCtrl mcctrl = { lastRegisteredMulticastId, // Multicast ID
                                    0x00,
                                    access_key, // Access key
                                    0x00 }; // Result
  
  *g_replymcframe = mcframe;
  *g_replymcctrl = mcctrl;
  protocolDispSendPacket(&aplink,(void*)&g_replybuffer,sizeof(g_replybuffer));
  if (!waitReply(messageSeq, 1000)) 
    printw("%s(): Failed to receive reply for messageId %u (Timed out)!\n", 
           __FUNCTION__, messageSeq);     
  
  printw("%s(): Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));   


}
//*****************************************************************************

static void test_Multicast(void)
{
  static unsigned int repeat = 1;
  static unsigned int interval_ms = 100;
  printw("Send Multicast:\n");
  printw("MulticastId? [0x%08x]: ", lastRegisteredMulticastId);
  scanw("%lx", &lastRegisteredMulticastId);
  
  printw("Access_Key? [0x%016llx]: ", access_key);
  scanw("%llx", &access_key);

  char newpayload[sizeof(mcast_payload)];
  printw("Payload? [Have %u bytes]: ", mcast_payload_size);
  getnstr(newpayload, sizeof(newpayload) );
  const unsigned int newpayloadsize = strlen(newpayload);  
  if (newpayloadsize > 0) {
    mcast_payload_size = newpayloadsize+1;
    memcpy(mcast_payload, newpayload, mcast_payload_size);
  }

  printw("Repeat? [%u times]: ", repeat);
  scanw("%u", &repeat);

  if (repeat > 1) {
    printw("Interval? [%u ms]: ", interval_ms);
    scanw("%u ", &interval_ms);
  }


  halfdelay(1);
  
  const double begin = ownTime_get_s();

  //
  

  // Construct Query
  const unsigned int multicast_testbuffer_size = ( sizeof(TFastProtocolHeader) + 
                                                   sizeof(TAPMulticastFrame) + 
                                                   sizeof(TAPMulticastPayload) + 
                                                   mcast_payload_size );
  uint8_t * multicast_testbuffer = (uint8_t *) malloc(multicast_testbuffer_size);
  assert(multicast_testbuffer != NULL);


  TFastProtocolHeader *fpframe = (TFastProtocolHeader*)multicast_testbuffer;
  TAPMulticastFrame *mcframe = (TAPMulticastFrame *)(multicast_testbuffer+sizeof(TFastProtocolHeader));
  TAPMulticastPayload *mcpl = (TAPMulticastPayload *)(multicast_testbuffer+sizeof(TAPMulticastFrame)+sizeof(TFastProtocolHeader));
  

  // Setup FastProtocolFrame
  fpframe->source_id = 0x00;
  fpframe->target_id = targetNodeId;
  fpframe->flags = 0x00;
  fpframe->ttl = FASTPROTOCOL_DEFAULT_TTL;
  fpframe->statusvalue = 0x00;
  fpframe->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  fpframe->protocol_id.total_size = multicast_testbuffer_size;

  // Setup MulticastFrame
  mcframe->service_id = GIMNETAP_SERVICE_MULTICAST_ID;
  mcframe->request = KAPMulticastRequestMulticast;
  mcframe->identifier = ++messageSeq;
  
  // Setup payload header
  mcpl->multicast_id = lastRegisteredMulticastId;
  mcpl->access_key = access_key;
  
  // copy payload
  assert(mcast_payload_size < sizeof(mcast_payload));
  memcpy(mcpl->payload, mcast_payload, mcast_payload_size);

  // Send!
  int repeat_count;
  for(repeat_count=0; repeat_count < repeat; ++repeat_count) {
    if (repeat_count > 0) ownSleep_ms(interval_ms);
    printw("Sending FastProtocol frame total of %u bytes long\n", multicast_testbuffer_size);
    protocolDispSendPacket(&aplink,(void*)multicast_testbuffer,multicast_testbuffer_size);
  }
  
  printw("%s(): (Blind) Operation completed in %.2f ms\n", 
         __FUNCTION__, 1000.0*(ownTime_get_s()-begin));  

}
//*****************************************************************************

static int commandHandler(int value)
{
  int r = 1;
  printw("\n");
  switch(value) {

  case 't':
    printw("TargetNodeId: ");
    if (scanw("%x", &targetNodeId) == 1) {
      g_replyfpframe->target_id = targetNodeId;
      printw("Using TargetNodeId: 0x%08x\n", targetNodeId); 

    } else {
      printw("Invalid TargetNodeId, must be a hex value\n");

    }
    halfdelay(1);
    break;
    
  case '1':
    test_TestSeq1();
    break;

  case 'c':
    // Create MulticastID
    test_CreateMulticastId();
    break;

  case 'p':
    // Publish Public
    test_PublishPublic();
    break;

  case 'P':
    // Publish private
    test_PublishPrivate();
    break;
    
  case 's':
    // Subscribe
    test_Subscribe();
    break;
    
  case 'm':
    // Multicast!
    test_Multicast();
    break;


  case 'u':
    // Unsubscribe
    break;

  case 'U':
    // Unpublish
    break;

    /*  case 'i':
    // Query Id (reverseQuery)
    test_ReverseQuery();
    break;*/

  case 'q':
    printw("Terminate program...\n");
    r = 0;
    break;

  case 'h':
    printw("\nAvailable Tests:\n");
    printw(" 't'       Set TargetNodeId\n");
    printw(" 'c'       Create new MulticastID\n");
    printw(" 'p'       Publish Public\n");
    printw(" 'P'       Publish Private\n");
    printw(" 's'       Subscribe to Multicast\n");
    printw(" 'm'       Test Multicast\n");
    //    printw(" 'i'       Query Id (Reverse query)\n");
    //    printw(" 'i'       Query Id (Reverse query)\n");
    printw(" 'q'       Quit\n");
    printw("\n");
    break;
  }
  return r;
}
//*****************************************************************************

static int ExecuteService(TProtocolDisp *link)
{
  int result = 0;

  // Init sync mechanisms
  lock = ownMutex_Init();
  cond = ownCond_Init();
  

  // Register callback,
  result = protocolDispRegisterCallback(link, FASTPROTOCOL_NUMBER, 
                                        fastProtocolCallback, NULL);
  result |= protocolDispRegisterCallback(link, HUBCTRLPROTOCOL_NUMBER, 
                                         hubctrlProtocolCallback, NULL);
  assert(result == 0);

  // Setup the default FastProtocol carrier frame
  memset(g_replyfpframe, 0x00, sizeof(TFastProtocolHeader));
  g_replyfpframe->source_id = 0x00;
  g_replyfpframe->target_id = targetNodeId;
  g_replyfpframe->flags = 0x00;
  g_replyfpframe->ttl = FASTPROTOCOL_DEFAULT_TTL;
  g_replyfpframe->statusvalue = 0x00;
  g_replyfpframe->protocol_id.protocol_number = FASTPROTOCOL_NUMBER;
  g_replyfpframe->protocol_id.total_size = sizeof(g_replybuffer);

  // Send reques for own id
  requestOwnId(link);

  // Init curses
  w = initscr(); 
  cbreak(); 
  //  noecho();
  nonl();
  intrflush(stdscr, FALSE);
  keypad(stdscr, TRUE);
  //nodelay(w, 1);
  halfdelay(1);
  atexit( (void (*)(void))endwin );

  int value;

  // Start processor
  protocolDispProcessorThreadHandle = ownThread_Create((void*)protocolDispProcessorThread, link);

  while (run) {
    showwait();
    
    // Read input
    value = getch();
    
    // Handle input
    if (value != ERR) {
      clear();
      refresh();

      run = commandHandler(value);
    }
  }
  return result;
}
//*****************************************************************************

static void showUsage()
{
}
//*****************************************************************************

static int SignalHandlerThread(void *t)
{
  dPrint(8, "Signal handler thread (%ld) starting...", pthread_self());

  while (run) {
    // Now, wait for requested signals to arrive
    int sig;
    sigset_t mask;
    sigemptyset(&mask);
    sigaddset(&mask, SIGINT);
    sigaddset(&mask, SIGTERM);
    sigaddset(&mask, SIGPIPE);
    sigaddset(&mask, SIGHUP);
    sigwait(&mask, &sig);

    // Got signal, sigwait returned!
    dPrint(ODSTATUS,"Got Signal '%d' (%s)", sig, strsignal(sig));

    // What to do?
    switch(sig) {
    case SIGINT:
    case SIGTERM:
    case SIGPIPE:
    case SIGHUP:
      run = 0;
      ownThread_Signal(signalHandlerThread, SIGINT);
      ownSleep_ms(1000);
      break;
      
    default:
      dPrint(ODSTATUS,"Unhandled signal! Ignore!");
      break;
    }
  }
  return 0;
}
//*****************************************************************************

static void InitSignalHandler(void)
{
  // Now; Set to BLOCK ALL SIGNALS
  sigset_t mask;
  sigemptyset(&mask);
  sigaddset(&mask, SIGINT);
  sigaddset(&mask, SIGTERM);
  sigaddset(&mask, SIGPIPE);
  sigaddset(&mask, SIGHUP);
  pthread_sigmask(SIG_BLOCK, &mask, NULL);
  
  // Start signal handler thread
  signalHandlerThread = ownThread_Create((void*)SignalHandlerThread, NULL);
}
//*****************************************************************************

int main(int argc, char *argv[]) 
{
  int paramParseLoop = 1;
  //  unsigned int service_address = 0x00000001;
  //  const char *authenticationFile = NULL;
  char targetAddress[255] = "localhost";
  int targetTcpPort = 10000; // Port to use
  int r = 0;

  
  // Set up utils (debugs mostly)
  debugInit();
  debugSetGlobalDebugLvl(ODINFO);
  debugSetLogFilename("multicast_test.log");
  debugRemoveMode( DEBUG_MODE_TIMESTAMP_HH_MM_SS | DEBUG_MODE_TIMESTAMP_HH_MM_SS_MS );
  debugAddMode( DEBUG_MODE_TIMESTAMP_DELTA_SS_US );

  // Now; Set to BLOCK ALL SIGNALS
  InitSignalHandler();
  
  // Print version
  printf("\nGIMnet Multicast TestApplication - %s\n\n", GIMNETAP_MULTICAST_TESTAPP_VERSIONSTRING);
  
  
  // Decode command line parameters.
  while(paramParseLoop) {
    int c;
    c = getopt(argc, argv, "u:p:qvh");
    switch(c) {
      
    case 'p':
      // Port to use
      targetTcpPort = atoi(optarg);
      break;
      
    case 'u':
      // Host to
      strncpy(targetAddress, optarg, sizeof(targetAddress)-1);
      targetAddress[sizeof(targetAddress)-1] = '\0';
      break;
      
      //////////////////////////////////
      //// GENERIC:                 ////
      //////////////////////////////////

    case 'v':
      // Increase only if not previously set to 0 (by 'quiet')
      if (verbose > 0) verbose++;
      debugSetGlobalDebugLvl(verbose);
      break;
      
    case 'q':
      verbose = 0;
      debugSetGlobalDebugLvl(verbose);
      debugSetLogFilename(NULL);
      break;
      
    case 'h':
    case '?':
      showUsage();
    exit(1);
    break;
    
    case -1:
      paramParseLoop = 0;
      break;
    }
  }

  // Now; got all parameters -> start processing
  // 1. Initialize TCP link
  int v = 0;
  v = protocolDispInit(&aplink);
  dPrint(ODTEST,"Connecting to '%s' at port %u", targetAddress, targetTcpPort);
  v &= protocolDispConnect(&aplink, targetAddress, targetTcpPort);
  if (v == PROTOCOLDISPATCHER_OK) {
    // OK!
    r = ExecuteService(&aplink);

  } else {
    // FAILED!
  } 

  printf("tcpHub exiting.\r\n");

  return r;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
